/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/timer.cpp
 * @Description  : 实现了一个高精度定时器系统，支持多任务管理。
 *                 定时器可以设置为软定时器或硬定时器，通过信号处理和线程同步来管理定时任务的执行。
 *                 代码提供了定时器的启动、停止、任务分配、使能、禁用等功能。
 *                 定时任务通过回调函数执行，支持多任务并行处理，并且具有任务使能和退出检查功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:02:42
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include <stdio.h> 
#include <time.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <unistd.h>
#include "timer.h"

HANDLE timer_calling; // Event to indicate the MM timer is calling

///////////////////////////////////////////////////////////////////////////////
//   The callback functions for the timer.
/**
 * @brief 处理信号的时间处理函数
 *
 * 根据接收到的信号执行相应的操作。
 *
 * @param signo 接收到的信号编号
 */
void TimeProc(int signo)
{
    switch (signo)
    {
    case SIGUSR1:
        AddSem(timer_calling);
        break;
    default:
        break;
    }
}
// void TimerThread(union sigval v)
//{
//     AddSem(timer_calling);
// }
//
//    The thread procedure for the deferred call.
//
/**
 * @brief 定时器线程处理函数
 *
 * 该函数是一个线程处理函数，用于执行定时器任务。
 *
 * @param p_timer 指向Timer对象的指针，用于访问定时器相关参数和任务
 *
 * @return void* 返回NULL
 */
void* TimerThreadProc(LPVOID p_timer)
{
    Timer* timer = static_cast<Timer*>(p_timer);
    // Loop until the kill event is set
    while (WaitforThreadSignal(timer->kill_timer_thread_, 0) != WAIT_OBJECT_0) {
        // DEBUG_OUT("TimerThreadProc");
        TIME_TYPE begin = GetTickCount();
        {
            std::lock_guard<std::mutex> locker(timer->function_mutex_);
            for (int i = 0; i < NUM_TIMER_TASKS; i++)
                timer->tasks_[i].Run();
        }

        TIME_TYPE end = GetTickCount();
        int elapsed_ms = end - begin;

        if (!timer->soft_timer_) {
            sem_wait(timer_calling); // 50ms try sem_t from 1 to 0; Block Wait
        } else {
            //  Sleep( param->m_uPeriod); //52ms
            int period = timer->period_; //
            int diff = period - elapsed_ms;
            if (diff < 0) {
                DEBUG_ERROR_OUT("error:TimerThreadProc: cycleTime is too long; elapsed_ms=" << (int)(elapsed_ms));
            } else {
                DEBUG_OUT("TimerThreadProc: cycleTime diff=" << (int)(diff));
                Sleep(diff); // 52ms
            }
        }
        // DEBUG_OUT("timer task is run...time="<<GetTickCount());
    }

    AddSem(timer->timer_thread_dead_);
    pthread_exit(nullptr);
    return nullptr;
}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CTimerTask".
//
//   Set function for the timer task.
//
void TimerTask::SetFunction(void (*function)(DWORD), DWORD param, bool enable) 
{
    function_ = function;
    param_ = param;
    enabled_ = enable;
    exit_ = false;
}

//
//   Clear the timer task.
//
void TimerTask::Clear() 
{
    function_ = nullptr;
    param_ = 0;
    enabled_ = false;
}

//
//   Enable/Disable the timer task.
//
void TimerTask::Enable(bool enable) 
{
    enabled_ = enable;
}

//
//   Test whether the task exists or not.
//
bool TimerTask::Exists() const 
{
    return function_ != nullptr;
}

//
//   Test whether the task is enabled.
//
bool TimerTask::IsEnabled() const 
{
    return Exists() && enabled_;
}

void TimerTask::UpdateMultiplier(int multiplier)
{
    timer_multiplier_ = multiplier;
    InitCounter();
}

void TimerTask::UpdateCounter() 
{
    if (timer_multiplier_ == 1) {
        multiplier_on_target_ = true;
        return;
    }
    timer_counter_++;
    if (timer_counter_ == timer_multiplier_) {
        multiplier_on_target_ = true;
        timer_counter_ = 0;
    }
}

void TimerTask::InitCounter() 
{
    timer_counter_ = 0;
    multiplier_on_target_ = true;
}

//
//   Run the timer task.
//
void TimerTask::Run() 
{
    if (IsEnabled()) {
        function_(param_);
    }
    if (function_ == nullptr && !exit_) {
        exit_ = true;
    }
}

//
//   Check whether the task exit
//
bool TimerTask::CheckExit() const 
{
    return exit_;
}
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CTimer"
//
//   Set the period for the timer
//
bool Timer::SetPeriod(unsigned int period) 
{
    if (started_ || period == 0) return false;
    period_ = period;
    return true;
}
//
//   Start the timer.
//
bool Timer::Start(bool soft_timer) 
{
    if (period_ == 0 || started_ || thread_ != 0) return false;
    soft_timer_ = soft_timer;
    kill_timer_thread_ = CreateSem(false, false);
    timer_thread_dead_ = CreateSem(false, false);
    timer_calling = CreateSem(false, false);
    if (kill_timer_thread_ == nullptr || timer_thread_dead_ == nullptr || timer_calling == nullptr) return false;

    pthread_attr_t attr;
    SetPthreadPriority(SCHED_RR, THREAD_PRIORITY_HIGHEST, attr);
    if (pthread_create(&thread_, &attr, TimerThreadProc, this) != 0) {
        DEBUG_OUT("Create TimerThreadProc Pthread Failed");
        return false;
    }
    DEBUG_OUT("Create TimerThreadProc Pthread OK");
    pthread_attr_destroy(&attr);

    if (!soft_timer_) {
        if (!SetHighResolutionTimer(period_)) {
            return false;
        }
    }

    started_ = true;
    return true;
}

/**
 * @brief 设置高精度定时器
 *
 * 设置一个高精度定时器，以指定的频率（毫秒）触发定时事件。
 *
 * @param frequency_ms 定时器的触发频率（毫秒）
 *
 * @return 设置成功返回 true，失败返回 false
 */
bool Timer::SetHighResolutionTimer(unsigned long frequency_ms) 
{
    struct sigaction act;
    memset(&act, 0, sizeof(act));
    act.sa_handler = TimeProc;
    act.sa_flags = 0;
    sigemptyset(&act.sa_mask);
    if (sigaction(SIGUSR1, &act, nullptr) == -1) {
        DEBUG_ERROR_OUT("fail to sigaction");
        return false;
    }

    struct sigevent evp1;
    memset(&evp1, 0, sizeof(struct sigevent));
    evp1.sigev_signo = SIGUSR1;
    evp1.sigev_notify = SIGEV_SIGNAL;
    if (timer_create(CLOCK_REALTIME, &evp1, &high_res_timer_) == -1) {
        DEBUG_ERROR_OUT("fail to timer_create");
        return false;
    }

    struct itimerspec its;
    memset(&its, 0, sizeof(struct itimerspec));
    unsigned long frequency_ns = frequency_ms * 1000 * 1000;
    its.it_value.tv_sec = frequency_ns / 1000000000;
    its.it_value.tv_nsec = frequency_ns % 1000000000;
    its.it_interval.tv_sec = its.it_value.tv_sec;
    its.it_interval.tv_nsec = its.it_value.tv_nsec;
    if (timer_settime(high_res_timer_, 0, &its, nullptr) == -1) {
        DEBUG_ERROR_OUT("fail to timer_settime");
        return false;
    }
    return true;
}

//
//   Stop the current session.
//
bool Timer::Stop() 
{
    if (!started_) return false;
    AddSem(kill_timer_thread_);
    if (!pthread_join(thread_, nullptr)) {
        pthread_cancel(thread_);
    } else {
        pthread_cancel(thread_);
        perror("pthread_join timer_pro error");
    }

    timespec sttimespec;
    sttimespec.tv_sec = 5;
    sttimespec.tv_nsec = 0;
    sem_timedwait(timer_thread_dead_, &sttimespec);

    if (!soft_timer_) {
        timer_delete(high_res_timer_);
    }

    DestroySem(kill_timer_thread_);
    DestroySem(timer_thread_dead_);
    DestroySem(timer_calling);
    if (kill_timer_thread_ != nullptr) {
        delete kill_timer_thread_;
        kill_timer_thread_ = nullptr;
    }

    if (timer_thread_dead_ != nullptr) {
        delete timer_thread_dead_;
        timer_thread_dead_ = nullptr;
    }

    if (timer_calling != nullptr) {
        delete timer_calling;
        timer_calling = nullptr;
    }

    started_ = false;
    return true;
}

//
//   Enable timer.
//
void Timer::Enable() {
    // AddSem(enable_timer_);
}

//
//   Disable the timer.
//
void Timer::Disable() {
    // ReAddSem(enable_timer_);
}

void Timer::UpdateMultiplier(int task_id, int multiplier) 
{
    tasks_[task_id].UpdateMultiplier(multiplier);
}
//
//   Assign a task for the specified timer task
//
bool Timer::AssignTask(short task_id, void (*task_routine)(DWORD), DWORD param, bool enable) 
{
    if (task_id < 0 || task_id >= NUM_TIMER_TASKS) return false;
    std::lock_guard<std::mutex> locker(mutex_);
    tasks_[task_id].SetFunction(task_routine, param, enable);
    return true;
}

//
//   Clear the specified timer task.
//
bool Timer::ClearTask(short task_id) 
{
    if (task_id < 0 || task_id >= NUM_TIMER_TASKS) return false;
    std::lock_guard<std::mutex> locker(mutex_);
    if (task_id == -1) {
        for (short i = 0; i < NUM_TIMER_TASKS; ++i) {
            tasks_[i].Clear();
        }
    } else {
        tasks_[task_id].Clear();
    }
    return true;
}

//
//   Enable the specified timer task.
//
bool Timer::EnableTask(short task_id, bool enable) 
{
    if (task_id < 0 || task_id >= NUM_TIMER_TASKS) return false;
    std::lock_guard<std::mutex> locker(mutex_);
    tasks_[task_id].Enable(enable);
    return true;
}

//
//   Test whether the task is enabled or not.
//
bool Timer::IsTaskEnabled(short task_id)
{
    if (task_id < 0 || task_id >= NUM_TIMER_TASKS) return false;
    std::lock_guard<std::mutex> locker(mutex_);
    return tasks_[task_id].IsEnabled();
}

//
//   Reset the timer tick.
//
// void CTimer::ResetTick()
// {
//     m_crit.Lock();
//     m_ulTick = 0;
//     m_crit.Unlock();
// }

//
//   Get the current tick value.
//
// ULONG CTimer::GetTick()
// {
//     m_crit.Lock();
//     ULONG ulTemp = m_ulTick;
//     m_crit.Unlock();

//     return ulTemp;
// }

//
//   Check whether the specified task exit
//
bool Timer::CheckExit(short task_id) const 
{
    return tasks_[task_id].CheckExit();
}